/*
******************************************************************************************************* 
**  Copyright (C) 2019, 苏州检易生物科技有限公司 
**  All rights reserved. 
** 
**  FileName:       	motor.c
**  Description:	
**  Author:        	 
**  Version				v0.1
**  Date:           	2019-06-29
**	Function List： 
**  History:         
*********************************************************************************************************
*/


/*
*********************************************************************************************************
*                                              	调试开关
*********************************************************************************************************
*/

/*
*********************************************************************************************************
*                                             INCLUDE FILES
*********************************************************************************************************
*/
#include "system_cfg.h"
#include "can_mem.h"
#include "can_task.h"
#include "schedule_lib.h"
#include "motor_step.h"
#include "motor_process.h"
#include "act.h"
#include "process_tmr.h"
#include "timer.h"
/*
*********************************************************************************************************
*                                              	宏定义
*********************************************************************************************************
*/



/*
*********************************************************************************************************
*                                              	结构体定义
*********************************************************************************************************
*/

 
/*
*********************************************************************************************************
*                                              	函数原型声明
*********************************************************************************************************
*/

/*
*********************************************************************************************************
*                                              	全局变量定义
*********************************************************************************************************
*/

/*
*********************************************************************************************************
*                                              	模块静态变量定义
*********************************************************************************************************
*/
static uint8_t	config_func(void* p_act);
static uint8_t	circle_func(void* p_act);
static uint8_t	home_func(void* p_act);
static uint8_t	halt_func(void* p_act);
static uint8_t	speed_func(void * p_act);
static uint8_t	pos_func(void * p_act);
static uint8_t	step_func(void * p_act);
static uint8_t	surface_func(void * p_act);
static uint8_t	wait_func(void* p_act);
static uint8_t	syn_func(void* p_act);
static uint8_t	write_tar_func(void* p_act);
static uint8_t	release_func(void* p_act);
static uint8_t	reverse_func(void* p_act);
static uint8_t	surface_calib_func(void* p_act);
static uint8_t	surface_wait_calib_func(void* p_act);
static uint8_t	collision_protect_func(void* p_act);
static uint8_t	check_surface_func(void* p_act);

static act_t*		motor_act_new(motor_t* p_motor);
static motor_t*	check_motor_en(uint8_t morot_id);

static void motor_cmd_push(motor_t* p_motor,sdo_cs_t rw,motor_reg_t reg,int32_t data);
/*********************************************************************************************************
*                                              	函数定义
*********************************************************************************************************
*/

/*配置电机模式 速度 只在初始化时调用一次 其他情况调用会出问题*/
uint8_t	motor_config(uint8_t motor_id,void* p_schedule_cb,motor_mode_t mode)
{
	ASSERT(p_schedule_cb);
	ASSERT(mode == M_MODE_POS || mode == M_MODE_SPEED ||mode == M_MODE_SPEED_RV);
	motor_t* p_motor = id_get_motor(motor_id);
	if(p_motor == NULL)
		return 0;
	p_motor->p_task_cb = p_schedule_cb;
	if(p_motor->p_motor_config->enable == MOTOR_DISABLE)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	if((uint32_t)p_motor->p_motor_config->speed > MOTOR_SPEED_LIMIT)
		p_act->arg[ACT_PARA_SPEED_INDEX] 		= MOTOR_SPEED_LIMIT;
	else
		p_act->arg[ACT_PARA_SPEED_INDEX] 		= (int32_t)p_motor->p_motor_config->speed;
	p_act->arg[ACT_PARA_ARG_INDEX] = (int32_t)mode;
	if(mode == M_MODE_SPEED && p_motor->p_motor_config->dir)
		p_act->arg[ACT_PARA_ARG_INDEX] = M_MODE_SPEED_RV;
	else if(mode == M_MODE_SPEED_RV && p_motor->p_motor_config->dir)
		p_act->arg[ACT_PARA_ARG_INDEX] = M_MODE_SPEED;
	
	p_act->func										 = config_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
/*配置电机整圈循环模式*/
uint8_t motor_circle(uint8_t motor_id,int32_t circle_step)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->arg[ACT_PARA_ARG_INDEX] 	= (int32_t)circle_step;
	p_act->func										 		= circle_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}

/*使速度模式下的电机反转*/
uint8_t motor_reverse(uint8_t motor_id)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	if((uint32_t)p_motor->p_motor_config->speed > MOTOR_SPEED_LIMIT)
		p_act->arg[ACT_PARA_SPEED_INDEX] 		= MOTOR_SPEED_LIMIT;
	else
		p_act->arg[ACT_PARA_SPEED_INDEX] 		= (int32_t)p_motor->p_motor_config->speed;
	p_act->func										 = reverse_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
/*配置电机默认速度，对于速度模式 则直接开始运动*/
uint8_t motor_speed(uint8_t motor_id)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	if((uint32_t)p_motor->p_motor_config->speed > MOTOR_SPEED_LIMIT)
		p_act->arg[ACT_PARA_SPEED_INDEX] 		= MOTOR_SPEED_LIMIT;
	else
		p_act->arg[ACT_PARA_SPEED_INDEX] 		= (int32_t)p_motor->p_motor_config->speed;
	p_act->func										 = speed_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
/*电机去使能*/
uint8_t motor_release(uint8_t motor_id,void* p_schedule_cb)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	p_motor->p_task_cb = p_schedule_cb;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 = release_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
/*速度模式下电机特例 速度 正反转 在位置模式下 则配置电机特殊运动时的速度*/
uint8_t motor_speed_ex(uint8_t motor_id,int16_t rpm)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	if((uint32_t)p_motor->p_motor_config->speed > MOTOR_SPEED_LIMIT)
		p_act->arg[ACT_PARA_SPEED_INDEX] 		= MOTOR_SPEED_LIMIT;
	else
		p_act->arg[ACT_PARA_SPEED_INDEX] 	= (int32_t)rpm;
	p_act->func										 		= speed_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
/*将速度写为0 刹车*/
uint8_t motor_halt(uint8_t motor_id)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 		= halt_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
//	if(p_motor->mode == M_MODE_POS)
//	{
//		motor_syn(motor_id);
//		motor_write_tar(motor_id);
//		motor_wait(motor_id);
//	}
//	else
//	{
//		motor_wait(motor_id);
//	}
	return 0;
}
/*电机归零*/
uint8_t motor_home(uint8_t motor_id)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 		= home_func;
	p_act->arg[ACT_PARA_ARG_INDEX] 		= 0;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	motor_wait(motor_id);
	return 0;
}
uint8_t motor_home_mode(uint8_t motor_id,uint8_t mode)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 		= home_func;
	p_act->arg[ACT_PARA_ARG_INDEX] 		= (int32_t)mode;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	motor_wait(motor_id);
	return 0;
}
/*同步电机当前步数,等待电机空闲*/
uint8_t motor_wait(uint8_t motor_id)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 		= wait_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
/*电机绝对位置运动*/
uint8_t motor_pos(uint8_t motor_id, uint8_t next_pos,uint32_t arg)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->arg[ACT_PARA_ARG_INDEX] 		= (int32_t)arg;
	p_act->arg[ACT_PARA_POS_INDEX] 		= (int32_t)next_pos;
	p_act->func										 		= pos_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	motor_wait(motor_id);
	return 0;
}
/*电机绝对位置运动*/
uint8_t motor_pos_asyn(uint8_t motor_id,uint8_t next_pos,uint32_t arg)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->arg[ACT_PARA_ARG_INDEX] 		= (int32_t)arg;
	p_act->arg[ACT_PARA_POS_INDEX] 		= (int32_t)next_pos;
//	p_act->arg[ACT_PARA_SYNC_INDEX] 		= (int32_t)MOTOR_ASYN;
	p_act->func										 		= pos_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
/*电机相对位置运动*/
uint8_t motor_step(uint8_t motor_id,uint8_t next_pos,uint32_t arg)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->arg[ACT_PARA_ARG_INDEX] 		= (int32_t)arg;
	p_act->arg[ACT_PARA_POS_INDEX] 		= (int32_t)next_pos;
	p_act->func										 		= step_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	motor_wait(motor_id);
	return 0;
}
/*页面探测*/
uint8_t motor_surface(uint8_t motor_id,uint8_t next_pos,uint32_t surface_mode,uint32_t arg)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->arg[ACT_PARA_ARG_INDEX] 		= (int32_t)arg;
	p_act->arg[ACT_PARA_EXTRA_INDEX] 	= (int32_t)surface_mode;
	p_act->arg[ACT_PARA_POS_INDEX] 		= (int32_t)next_pos;
	p_act->func										 		= surface_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	motor_wait(motor_id);
	motor_syn(motor_id);
	motor_check_surface(motor_id);
	return 0;
}
/*将驱动板的当前位置同步至上位机*/
uint8_t motor_syn(uint8_t motor_id)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 		= syn_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
uint8_t motor_check_surface(uint8_t motor_id)/*检查液面探测状态 接在motor_syn后面*/
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 		= check_surface_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
uint8_t motor_write_tar(uint8_t motor_id)
{	
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 		= write_tar_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
uint8_t motor_surface_calib(uint8_t motor_id)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 		= surface_calib_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
uint8_t motor_surface_wait_calib(uint8_t motor_id)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->func										 		= surface_wait_calib_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
/*使能电机限位光电,主要用于针防碰撞*/
uint8_t motor_collision_protect(uint8_t motor_id,uint32_t type)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor == NULL)
		return 0;
	act_t* p_act = motor_act_new(p_motor);
	if(p_act == NULL)
	{
		return 1;
	}
	p_act->arg[ACT_PARA_ARG_INDEX] 		= (int32_t)type;
	p_act->func										 		= collision_protect_func;
	list_push(&((schedule_cb_t*)p_motor->p_task_cb)->send_list, p_act);
	return 0;
}
/*
*********************************************************************************************************
*                                              	静态函数定义
*********************************************************************************************************
*/
/*初始化电机配置 对于速度模式 先将速度写为零 然后写入模式
对于位置模式 写入模式 然后写入速度*/
static uint8_t	config_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	p_motor->mode = p_act->arg[ACT_PARA_ARG_INDEX];
	
	rt_base_t it = rt_hw_interrupt_disable();
	int32_t current = get_motor_current(p_motor->motor_id);
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_CURRENT,current);
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_MODE,p_motor->mode);
//#if MOTOR_HOME_TYPE_OLD
//#else
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_ACC_LEVEL,p_motor->p_motor_config->acc);
//#endif
	if(p_motor->mode == M_MODE_POS)
		motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SPEED,MOTOR_RPM_TO_DATA(p_motor->p_motor_config->speed));
	else
		motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SPEED,0);

	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	circle_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	rt_base_t it = rt_hw_interrupt_disable();
	
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_HALF_CIRCLE,p_act->arg[ACT_PARA_ARG_INDEX]);
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
/*将电机去使能*/
static uint8_t	release_func(void* arg)
{	
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	
	rt_base_t it = rt_hw_interrupt_disable();
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_CURRENT,0);
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	home_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	
	rt_base_t it = rt_hw_interrupt_disable();
	if(p_act->arg[ACT_PARA_ARG_INDEX] == 0)
	{
		if(p_motor->p_motor_config->mode_home == 0)/*没有复位参数时*/
		{
			Trace_Log("M%02d home arg can't be 0;\r\n",p_motor->motor_id);
			rt_hw_interrupt_enable(it);
			return ACT_RLT_DEL;
		}
		motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_HOME_MODE,p_motor->p_motor_config->mode_home);
	}
	else
		motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_HOME_MODE,p_act->arg[ACT_PARA_ARG_INDEX]);
	p_motor->status = MOTOR_STATE_HOMING;
	p_motor->tar_pos = 0;
	
	Trace_Log("M%02d homing;T:%d;\r\n",p_motor->motor_id,LocalTime);
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	halt_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	
	rt_base_t it = rt_hw_interrupt_disable();
//	if(p_motor->mode == M_MODE_POS)
//	{
////		can_mem_node_t* p_tar = can_mem_malloc();p_motor->sdo_cnt++;
////		motor_write(p_motor->p_motor_config,M_REG_TAR_POS,0,&p_tar->can_msg);
////		app_msg_send(CAN_MSG_PUSH,(uint32_t)p_tar,&g_can_task.mailbox);
////		
////		
////		can_mem_node_t* p_pos = can_mem_malloc();p_motor->sdo_cnt++;
////		motor_write(p_motor->p_motor_config,M_REG_POS,0,&p_pos->can_msg);
////		app_msg_send(CAN_MSG_PUSH,(uint32_t)p_pos,&g_can_task.mailbox);
//	}
//	else
//	{
//		motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SPEED,0);
//	}

	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_HALT,0);
	
	
//	p_motor->status = MOTOR_STATE_HALT;
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	speed_func(void * arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	
	rt_base_t it = rt_hw_interrupt_disable();
		motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_MODE,p_motor->mode);
		motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SPEED,MOTOR_RPM_TO_DATA(p_act->arg[ACT_PARA_SPEED_INDEX]));
	
	if(p_motor->mode == M_MODE_SPEED || p_motor->mode == M_MODE_SPEED_RV)
		p_motor->status = MOTOR_STATE_SPEED;
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	reverse_func(void * arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
//	if(p_motor->mode == M_MODE_POS)
//		return ACT_RLT_DEL;
//	else if(p_motor->mode == M_MODE_SPEED)
//		p_motor->mode = M_MODE_SPEED_RV;
//	else if(p_motor->mode == M_MODE_SPEED_RV)
//		p_motor->mode = M_MODE_SPEED;
	rt_base_t it = rt_hw_interrupt_disable();	
	if(p_motor->mode == M_MODE_SPEED)
		motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_MODE,M_MODE_SPEED_RV);
	else
		motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_MODE,M_MODE_SPEED);
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SPEED,MOTOR_RPM_TO_DATA(p_act->arg[ACT_PARA_SPEED_INDEX]));
	
	if(p_motor->mode == M_MODE_SPEED || p_motor->mode == M_MODE_SPEED_RV)
		p_motor->status = MOTOR_STATE_SPEED;
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	pos_func(void * arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	rt_base_t it = rt_hw_interrupt_disable();
	uint32_t cur_pos = p_motor->tar_pos;
	int32_t tar_pos = motor_pos_para(p_motor,p_act->arg[ACT_PARA_POS_INDEX],p_act->arg[ACT_PARA_ARG_INDEX]);
//	if(cur_pos == tar_pos)
//	{
//		rt_hw_interrupt_enable(it);
//		return ACT_RLT_DEL;
//	}
	Trace_Log("M%02d position %d;T:%d;\r\n",p_motor->motor_id,tar_pos,LocalTime);
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_TAR_POS,tar_pos);
	p_motor->status = MOTOR_STATE_STEP;
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
/*相对位置运动 先将速度写零 然后将当前位置写零 将目标位置写入 然后恢复速度*/
static uint8_t	step_func(void * arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	
	rt_base_t it = rt_hw_interrupt_disable();
	int32_t step_inc;
	step_inc = motor_step_para(p_motor,p_act->arg[ACT_PARA_POS_INDEX],p_act->arg[ACT_PARA_ARG_INDEX]);
	p_motor->tar_pos += step_inc; 
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_TAR_POS,p_motor->tar_pos);
	
	/*这种写法会导致位置不准*/
//	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SPEED,0);
//	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_TAR_POS,tar_step);
//	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_POS,0);
//	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SPEED,MOTOR_RPM_TO_DATA(p_motor->p_motor_config->speed));
	/*这种写法会导致停止位回复错误*/
//	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_POS,-tar_step);
	
	Trace_Log("M%02d step %d;T:%d;\r\n",p_motor->motor_id,step_inc,LocalTime);
	p_motor->status = MOTOR_STATE_STEP;
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
/*页面探测 先将液面探测寄存器使能 然后写入液面探测极限位置*/
static uint8_t	surface_func(void * arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	
	rt_base_t it = rt_hw_interrupt_disable();
	int32_t tar_pos = motor_pos_para(p_motor,p_act->arg[ACT_PARA_POS_INDEX],p_act->arg[ACT_PARA_ARG_INDEX]);
	Trace_Log("M%02d surface %d;T:%d;\r\n",p_motor->motor_id,tar_pos,LocalTime);
	
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SURFACE_ENABLE,1);
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_TAR_POS,tar_pos);
	
	p_motor->status = MOTOR_STATE_SURFACE;
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	surface_calib_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	rt_base_t it = rt_hw_interrupt_disable();
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SURFACE_INIT,1);
	p_motor->surface_status = P_INITING;
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	surface_wait_calib_func(void* arg)
{
	uint8_t rlt = ACT_RLT_NONE;
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	rt_base_t it = rt_hw_interrupt_disable();
	if(p_motor->surface_status == P_ERR)
	{
		Error_Log("Motor index %d surface catch err!\r\n",p_motor->motor_id);
		rlt = ACT_RLT_DEL;
	}
	else if(p_motor->surface_status == P_DETECTED)
	{
		Error_Log("Motor index %d surface detected during calib!\r\n",p_motor->motor_id);
		rlt = ACT_RLT_DEL;
	}
	else if(p_motor->surface_status == P_UNINIT)
	{
		Error_Log("Motor index %d surface need calib!\r\n",p_motor->motor_id);
		rlt = ACT_RLT_DEL;
	}
	else if(p_motor->surface_status == P_IDLE)
	{
		rlt = ACT_RLT_DEL;
	}
	rt_hw_interrupt_enable(it);
	return rlt;
}
static uint8_t	wait_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt == 0 && p_motor->status == MOTOR_STATE_IDLE)
		return ACT_RLT_DEL;
	else if(p_motor->sdo_cnt == 0 && p_motor->status == MOTOR_STATE_ERR)
	{
		if(p_motor->err_code == M_ERR_NONE)
		{
			rt_base_t it = rt_hw_interrupt_disable();
			motor_cmd_push(p_motor,SDO_CS_READ,M_REG_ERR,0);
			rt_hw_interrupt_enable(it);
		}
		return ACT_RLT_NONE;
	}
	return ACT_RLT_NONE;
}
static uint8_t	check_surface_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	if(p_motor->surface_status == P_DETECTED)
	{
		Trace_Log("M%02d surface detected;\r\n",p_motor->motor_id);
	}
	else
	{
#if PRODUCT_NAME_DESKTOP_200 | PRODUCT_NAME_IVD_400
		if(p_motor->motor_id == M_RP_VERT)
			tcp_err_push(MECH_ERR_RP_SURFACE,"M%02d surface detect err;status:%d;\n",p_motor->motor_id,p_motor->surface_status);
		else if(p_motor->motor_id == M_SP_VERT)
			tcp_err_push(MECH_ERR_SP_SURFACE,"M%02d surface detect err;status:%d;\n",p_motor->motor_id,p_motor->surface_status);
#endif
#if PRODUCT_NAME_IVD_400
		else if(p_motor->motor_id == M_MP_VERT)
			tcp_err_push(MECH_ERR_MP_SURFACE,"M%02d surface detect err;status:%d;\n",p_motor->motor_id,p_motor->surface_status);
#endif
	}
	return ACT_RLT_DEL;
}
static uint8_t	syn_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	
	rt_base_t it = rt_hw_interrupt_disable();
	
//	motor_cmd_push(p_motor,SDO_CS_READ,M_REG_SURFACE_STATUS,0); /*读取液面探测状态 然后再去使能M_REG_SURFACE_ENABLE*/
//	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_SURFACE_ENABLE,0);
	motor_cmd_push(p_motor,SDO_CS_READ,M_REG_POS,0);
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	write_tar_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	rt_base_t it = rt_hw_interrupt_disable();
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_TAR_POS,p_motor->tar_pos);
	
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static uint8_t	collision_protect_func(void* arg)
{
	act_t* p_act = (act_t*)arg;
	motor_t* p_motor = (motor_t*)p_act->arg[ACT_PARA_MOTOR_INDEX];
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	
	rt_base_t it = rt_hw_interrupt_disable();
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_COLLISION,p_act->arg[ACT_PARA_ARG_INDEX]);
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
static motor_t*	check_motor_en(uint8_t motor_id)
{
	motor_t* p_motor = id_get_motor(motor_id);
	if(p_motor == NULL ||
//		p_motor->status == MOTOR_STATE_ERR ||
		p_motor->p_motor_config->enable == MOTOR_DISABLE
		||((schedule_cb_t*)p_motor->p_task_cb) == NULL
		)
		return NULL;
	return p_motor;
}

static void motor_cmd_push(motor_t* p_motor,sdo_cs_t rw,motor_reg_t reg,int32_t data)
{
	ASSERT(p_motor);
	ASSERT(rw == SDO_CS_WRITE || SDO_CS_READ == rw);
	can_mem_node_t* p_node = can_mem_malloc();
	if(p_node == NULL)
	{
		Error_Log("Can mem alloc failed!\n");
		return;
	}
	p_motor->sdo_cnt++;
	if(rw == SDO_CS_WRITE)
		motor_write(p_motor->p_motor_config,reg,data,&p_node->can_msg);
	else
		motor_read(p_motor->p_motor_config,reg,&p_node->can_msg);
	app_msg_send(CAN_MSG_PUSH,(uint32_t)p_node,&g_can_task.mailbox);
}
static act_t*		motor_act_new(motor_t* p_motor)
{
	ASSERT(p_motor);
	ASSERT(p_motor->p_task_cb);
	act_t* p_act = list_new(&((schedule_cb_t*)p_motor->p_task_cb)->send_list);
	if(p_act == NULL)
	{
		Critical_Log("ACT node create failed!\r\n");
		return NULL;
	}
	memset(p_act, 0, sizeof(act_t));
	p_act->p_tcb 	= p_motor->p_task_cb;
	p_act->arg[ACT_PARA_MOTOR_INDEX] = (int32_t)p_motor;
	schedule_cb_t* p_task = p_motor->p_task_cb;
	return p_act;
}
uint8_t motor_check_idle(uint8_t motor_id)
{
	motor_t* p_motor = check_motor_en(motor_id);
	if(p_motor != NULL && p_motor->sdo_cnt == 0 && p_motor->status == MOTOR_STATE_IDLE)
		return 1;
	return 0;
}
uint8_t motor_disable(uint8_t motor_id)
{	
	Trace_Log("Motor %d disable!\r\n",motor_id);
	motor_t* p_motor = id_get_motor(motor_id);
	if(p_motor == NULL)
		return 0;
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	rt_base_t it = rt_hw_interrupt_disable();
	motor_cmd_push(p_motor,SDO_CS_WRITE,M_REG_DISABLE,1);
	rt_hw_interrupt_enable(it);
	return ACT_RLT_DEL;
}
int32_t motor_read_enc(uint8_t motor_id)
{
	motor_t* p_motor = id_get_motor(motor_id);
	if(p_motor == NULL)
		return 0;
	/*等待当前电机的SDO发送完成*/
	if(p_motor->sdo_cnt)
		return ACT_RLT_NONE;
	rt_base_t it = rt_hw_interrupt_disable();
	motor_cmd_push(p_motor,SDO_CS_READ,M_REG_ENC_POS,1);
	rt_hw_interrupt_enable(it);
	
	/*等待接受完成*/
	uint32_t timeout = 100;
	while(timeout--)
	{
		if(p_motor->sdo_cnt == 0) 
			return p_motor->enc_pos;
		rt_thread_delay(1);
	}
	return 0;
}